#include "types.h"
#include "interrupt_controller.h"
#include "uart.h"
#include "usb.h"
#include "sdcard_access.h"
#include "os_io.h"
#include "os_mem.h"
#include "loader.h"
#include "sw_interrupt_handler.h"
#include "sw_interrupt.h"
#include "psc.h"
#include "test_app.h"
#include "os_queue.h"
#include "os_timers.h"
#include "emac.h"
#include "os_dev.h"
#include "platform_i2c.h"
#include "cli.h"
#include "shell.h"
#include "perf_controller.h"
#include "flash_controller.h"

#include "test_suite.h"
#include "test_timers.h"
#include "ping.h"

extern uint32_t __usr__mem__base__;
extern uint32_t __usr__mem__len__;

extern void _test_mmu(uint32_t *page_table_addr, uint32_t *va_addr);




/* debug function */
uint32_t reached(uint32_t val)
{
  printf("Reached 0x%x\n", val);
  return 0;
}

void dump_stack(uint32_t *ptr) {
    uint8_t i = 0;
    printf("Stack pointer is: 0x%x\n", (uint32_t)ptr);
    for (i = 0; i < 17; i++) {
        printf("%x ", ptr[i]);
	if ((i & 0x4) == 0x4) { printf("\n"); }
    }
    printf("\n");
    return;
}
/* debug function end */


void test_my_flash(void)
{
    test_flash();
    //dump_flash_regs();
    return;
}

void setup_mmu(void)
{
  #define PAGE_TABLE_ADDRESS 0xC0E00000
  uint32_t *page_table_addr = (uint32_t *)PAGE_TABLE_ADDRESS;
  uint32_t *va_addr = (uint32_t *)0x00100000;
  uint32_t *pa_addr = (uint32_t *)0xC0F00000;

  *pa_addr = 0x12345678;

  /*
  * Section table base: 0xC0F
  * AP: 11 (read/write access in all modes)
  * Domain: 0xF (all domains to be configured with: allow access
  * C, B: 1, 1 currently
  */
  *(page_table_addr + 1) = 0xC0F00DFE;
  *(page_table_addr + 0xc30) = 0xC3000DFE; /* map VA=PA for code segment */

  printf("Going to test smu\n");
  _test_mmu(page_table_addr, va_addr);
  printf("Smu tested: 0x%x\n", *pa_addr);

  return;

}


void main(void)
{

	uart_init();
	initialize_interrupts();
	enable_interrupt(AINTC_UART_INTERRUPT);

	//enable_interrupt(16);
	//enable_interrupt(17);

	//debug_set_interrupt(16);
	uart_print((uint8_t *)"Going to pring clock regs\n");

	printf("Clock reg values: 0x%x, 0x%x\n",
	  *((uint32_t *)0x01c11148), *((uint32_t *)0x01c1114c));
	
	uart_print((uint8_t *)"Printed clock regs\n");

        printf("Enabling performance emulator\n");
	pm_init();

        printf("Initializing software interrupts\n");
	initialize_sw_interrupts();

	/*
	setup_mmu();
	while(1);
        */

	// Initialize other peripherals
	//psc_init();
	//sdcard_init();
	
	//usb11_init();		// Initialize USB1.1 host controller

        printf("Initializing user memory\n");
	user_mem_init();

        printf("Initializing loader\n");
	loader_init();

        printf("Initializing queues\n");
	init_queues_data();

	printf("Initializing timers\n");
	initialize_timers();

	printf("Enabling EMAC interrupts\n");
	initialize_emac_interrupts();

        printf("Initializing I2c bus\n");
        i2c_init(I2C0);

	printf("Initializing devices\n");
	init_devices();

	printf("Initializing cli\n");
	cli_init();

	printf("Initializing flash controller\n");
	//flash_init();



	//Save stack pointer and switch to user mode 
	uint32_t tmp = 0;
	uint32_t *sp = NULL;

	sp = create_os_process();	// sp shall reside in r0

	__asm__ __volatile__ (
	"mrs %[reg], cpsr\n\t"
	"bic %[reg], %[reg], #0xff\n\t"
	"orr %[reg], %[reg], #0x10\n\t"	/* switch to user mode, enable IRQ/FIQ */
	"msr cpsr, %[reg]\n\t\n\t"
	"mov sp, %[stack_ptr]\n\t"
	: [reg] "+r" (tmp): [stack_ptr] "r" (sp): "memory");


	/* Check if new sw interrupts work */
	printf("Verifying SW interrupts..\n");
	raise_sw_interrupt(SW_INT_DUMMY_INTERRUPT, 0x12345678);
	printf("Returned from dummy interrupt handler\n");

	printf("Initializing emac\n");
	emac_init();
	exec_mem(emac_txfun, (const uint8_t *)"emac_tx", 0, PROCESS_DRIVER_PRIORITY);
	exec_mem(emac_rxfun, (const uint8_t *)"emac_rx", 0, PROCESS_DRIVER_PRIORITY);

	printf("Initializing shell process\n");
	exec_mem(shell, (const uint8_t *)"shell", 0, PROCESS_OPERATOR_PRIORITY);
	
	printf("FLY initializaton complete\n");
	exec_mem(test_my_flash, (const uint8_t *)"test_flash", 0, PROCESS_USER_PRIORITY);


	//test_fly_os();

	start_icmp();


	while(1);
	//user_wait();


	// If returning to bootloader, disable interrupts
	// so that UART RX  is not double-processed
	DISABLE_INTERRUPTS();
	printf("Returning to bootloader\n");
	return;
}

